package com.td.core.obj.movement;

import java.util.LinkedList;

import com.td.core.obj.utils.TDVector;

public class Movement{

	private float speed;
	private TDVector target;
	private TDVector lastMovement = new TDVector();
	private LinkedList<TDVector> path;	
	private double rotation;
	private int stepScale;
	private float minDistanceToNextPoint;
	
	public Movement(float speed, LinkedList<TDVector> path) {
		
		this.speed = speed;
		this.path = path;

	}
	
	public Movement(Movement movement) {
		this(movement.speed, movement.path);
		this.target = movement.target;
		this.lastMovement = movement.lastMovement;
	}
	
	public TDVector calculate(TDVector position) {
		
	
		if(calculateNextTarget(position)) {
				
			TDVector movement = calculateMovement(position);
		   	 lastMovement.set(movement);
		   	 rotation = movement.getTheta();
		   	 	
		   	 return movement;
		   	 	
		}
		
		return null;
		
	}
	
	private boolean calculateNextTarget(TDVector position) {
		
		float minDistanceToPoint = minDistanceToNextPoint;
		if(target != null) {
			if(position.equals(target, minDistanceToPoint))
				return setNextTarget();
		} else
			return setNextTarget();
				
		return true;	
		
	}
		
	private boolean setNextTarget() {
		if(path != null && !path.isEmpty()) {
			target = path.getFirst();
			path.removeFirst();
			
			return true;
		}
		
		return false;
			
	}
	private TDVector calculateMovement(TDVector position) {
		
		TDVector movement = new TDVector(target);
		movement.sub(position).normalise().scale(speed);
                    
		double newTheta = movement.getTheta();
		double lastTheta = lastMovement.getTheta();
	             
		double thetaDif = lastTheta - newTheta;
		double step = stepScale;
             
		if(Math.abs(thetaDif) < step || Math.abs(thetaDif) > 360 - step)
			return movement;
             
		if(thetaDif > 180)
			thetaDif = thetaDif - 360;
		else if(thetaDif < -180)
			thetaDif = thetaDif + 360;
             
		step = thetaDif > 0 ? step : -step;
             
		movement.setTheta(lastTheta - step);
             
		return movement;
	}
     
	public void setStepScale(int stepScale) {
	
		this.stepScale = stepScale;
		
	}
	
	public double getRotation() {
		return rotation;
	}

	public void setMinDistanceToNextPoint(float minDistanceToNextPoint) {
		this.minDistanceToNextPoint = minDistanceToNextPoint;
	}	
	
	
}
